// --------------------------------------------------------------------------------------------------------------------
// <copyright file="IMUState.cs" company="Microsoft Corporation">
// The MIT License (MIT)
// 
// Copyright (c) 2014, Microsoft Corporation
// 
// Permission is hereby granted, free of charge, to any person obtaining a copy
//  of this software and associated documentation files (the "Software"), to deal
//  in the Software without restriction, including without limitation the rights
//  to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
//  copies of the Software, and to permit persons to whom the Software is
//  furnished to do so, subject to the following conditions:
// 
// The above copyright notice and this permission notice shall be included in
//  all copies or substantial portions of the Software.
// 
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
//  IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
//  FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
//  AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
//  LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
//  OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
//  THE SOFTWARE.
// </copyright>
// --------------------------------------------------------------------------------------------------------------------
namespace Microsoft.Robotics.Hardware.DAC
{
    using System;
    using System.Runtime.Serialization;

    using Microsoft.Robotics.Numerics;

    /// <summary>
    /// State of the IMU.
    /// </summary>
    [DataContract]
    public class IMUState : IDeviceState
    {
        /// <summary>
        /// Initializes a new instance of the <see cref="IMUState"/> class.
        /// </summary>
        public IMUState()
        {
        }

        /// <summary>
        /// Initializes a new instance of the <see cref="IMUState" /> class.
        /// </summary>
        /// <param name="requiredGyroMeasurementsForCalibration">Number of gyro readings to take before gyro is calibrated</param>
        /// <param name="gyroCalibrationThreshold">The threshold above which to ignore readings during gyro calibration</param>
        public IMUState(int requiredGyroMeasurementsForCalibration, double gyroCalibrationThreshold)
        {
            this.InitialGyroBiasCalibrationReadingCount = requiredGyroMeasurementsForCalibration;
            this.GyroBiasCalibrationReadingsRemaining = requiredGyroMeasurementsForCalibration;
            this.GyroCalibrationThreshold = gyroCalibrationThreshold;
        }

        /// <summary>
        /// Gets or sets the right-left acceleration measured in G forces
        /// </summary>
        [DataMember]
        public double AccelerationXInG { get; set; }

        /// <summary>
        /// Gets or sets the up-down acceleration measured in G forces
        /// </summary>
        [DataMember]
        public double AccelerationYInG { get; set; }

        /// <summary>
        /// Gets or sets the forwards-backwards acceleration measured in G forces
        /// </summary>
        [DataMember]
        public double AccelerationZInG { get; set; }

        /// <summary>
        /// Gets or sets the remaining number of gyro readings before gyro calibration is complete
        /// </summary>
        [DataMember]
        public int GyroBiasCalibrationReadingsRemaining { get; set; }

        /// <summary>
        /// Gets or sets the results of gyro calibration
        /// </summary>
        [DataMember]
        public Vector3 GyroBias { get; set; }

        /// <summary>
        /// Gets or sets the rotational velocity on the X axis measured in radians per second
        /// </summary>
        [DataMember]
        public double GyroXInRadiansPerSecond { get; set; }

        /// <summary>
        /// Gets or sets the rotational velocity on the Y axis measured in radians per second
        /// </summary>
        [DataMember]
        public double GyroYInRadiansPerSecond { get; set; }

        /// <summary>
        /// Gets or sets the rotational velocity on the Z axis measured in radians per second
        /// </summary>
        [DataMember]
        public double GyroZInRadiansPerSecond { get; set; }

        /// <summary>
        /// Gets or sets the threshold for values during calibration
        /// </summary>
        [DataMember]
        public double GyroCalibrationThreshold { get; set; }

        /// <summary>
        /// Gets or sets the number of gyro readings that must be made in order to calibrate the gyro bias
        /// </summary>
        [DataMember]
        public int InitialGyroBiasCalibrationReadingCount { get; set; }

        /// <summary>
        /// Gets or sets the timestamp for the state.
        /// </summary>
        [DataMember]
        public long TimestampInTicks { get; set; }

        /// <summary>
        /// Gets or sets the timestamp in milliseconds (as opposed to hardware-dependent ticks).
        /// </summary>
        [DataMember]
        public long TimestampInMilliseconds { get; set; }

        /// <summary>
        /// Gets or sets the device identifier
        /// </summary>
        [DataMember]
        public int DeviceId { get; set; }

        /// <summary>
        /// Gets or sets the device model information
        /// </summary>
        [DataMember]
        public string Model { get; set; }

        /// <summary>
        /// Gets or sets the type of the device the sensor is related
        /// </summary>
        [DataMember]
        public DeviceType DeviceType { get; set; }

        /// <summary>
        /// Gets or sets the pose for the device/sensor
        /// </summary>
        [DataMember]
        public Pose Pose { get; set; }

        /// <summary>
        /// Update state of IMU alternate contract.
        /// </summary>
        /// <param name="accelerometer">The latest accelerometer state</param>
        /// <param name="gyroscope">The latest gyroscope state</param>
        public void IMUHeartbeatUpdate(AccelerometerState accelerometer, GyroscopeState gyroscope)
        {
            // accelerometer
            if (accelerometer != null)
            {
                Vector3 accel = accelerometer.AccelerationInG;
                this.AccelerationXInG = accel.X;
                this.AccelerationYInG = accel.Y;
                this.AccelerationZInG = accel.Z;
            }

            // gyro (with n-initial sample average bias applied)
            bool calibrating = this.GyroBiasCalibrationReadingsRemaining > 0;
            Vector3 rotation = gyroscope.RotationalVelocityInRadiansPerSecond;
            Vector3 bias = this.GyroBias;

            this.GyroXInRadiansPerSecond = rotation.X - (calibrating ? 0 : bias.X);
            this.GyroYInRadiansPerSecond = rotation.Y - (calibrating ? 0 : bias.Y);
            this.GyroZInRadiansPerSecond = rotation.Z - (calibrating ? 0 : bias.Z);

            if (calibrating)
            {
                // ignore this whole reading if any readings are beyond the threshold
                if (Math.Abs(rotation.X) > this.GyroCalibrationThreshold || Math.Abs(rotation.Y) > this.GyroCalibrationThreshold || Math.Abs(rotation.Z) > this.GyroCalibrationThreshold)
                {
                    return;
                }

                bias = new Vector3(bias.X + rotation.X, bias.Y + rotation.Y, bias.Z + rotation.Z);

                this.GyroBiasCalibrationReadingsRemaining--;

                if (this.GyroBiasCalibrationReadingsRemaining == 0)
                {
                    int readingCount = this.InitialGyroBiasCalibrationReadingCount;
                    bias = new Vector3(bias.X / readingCount, bias.Y / readingCount, bias.Z / readingCount);

                    // BUG 1274: VirtualDriveFeedback should not publish messages while the gyro is calibrating
                    System.Console.WriteLine("Gyro calibrated {0}, {1}, {2}", bias.X, bias.Y, bias.Z);
                }

                this.GyroBias = bias;
            }
        }
    }
}
